﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace ScanDevConsole
{
    public static class ModbusHelp
    {
        /* CRC hsb */
        private static readonly byte[] auchCRCHi = { 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
        0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
        0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
        0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
        0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
        0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
        0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
        0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
        0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
        0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
        0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
        0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
        0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
        0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
        0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
        0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
        0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
        0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
        0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
        0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
        0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
        0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40 };

        /* CRC lsb*/
        private static readonly byte[] auchCRCLo = { 0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2,
        0xC6, 0x06, 0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD,
        0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8,
        0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F,
        0xDD, 0x1D, 0x1C, 0xDC, 0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6,
        0xD2, 0x12, 0x13, 0xD3, 0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1,
        0x33, 0xF3, 0xF2, 0x32, 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4,
        0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, 0x3B, 0xFB,
        0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA,
        0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5,
        0x27, 0xE7, 0xE6, 0x26, 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0,
        0xA0, 0x60, 0x61, 0xA1, 0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67,
        0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 0x6E, 0xAE,
        0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79,
        0xBB, 0x7B, 0x7A, 0xBA, 0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C,
        0xB4, 0x74, 0x75, 0xB5, 0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73,
        0xB1, 0x71, 0x70, 0xB0, 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92,
        0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C, 0x5D, 0x9D,
        0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98,
        0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F,
        0x8D, 0x4D, 0x4C, 0x8C, 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86,
        0x82, 0x42, 0x43, 0x83, 0x41, 0x81, 0x80, 0x40 };


        public static System.UInt16 crc16(ref byte[] data)   //计算输入数据 Modbus的校验
        {
            if (data.Length < 1) return 0;
            byte uchCRCHi = 0xFF;
            byte uchCRCLo = 0xFF;
            byte uIndex;

            foreach (byte b in data)
            {
                uIndex = BitConverter.GetBytes(uchCRCHi ^ b)[0];
                uchCRCHi = BitConverter.GetBytes(uchCRCLo ^ auchCRCHi[uIndex])[0];
                uchCRCLo = auchCRCLo[uIndex];
            }

            byte[] res = new byte[] { uchCRCLo, uchCRCHi };

            return BitConverter.ToUInt16(res, 0);
        }

        public static bool isPDU_CRC_OK(ref byte[] data)    //计算输入数据CRC 是否正确
        {
            if (data.Length < 3) return false;

            int len = data.Length - 2;
            byte uchCRCHi = 0xFF;
            byte uchCRCLo = 0xFF;
            byte uIndex;

            for(int i=0; i < len; i++)
            {
                uIndex = BitConverter.GetBytes(uchCRCHi ^ data[i])[0];
                uchCRCHi = BitConverter.GetBytes(uchCRCLo ^ auchCRCHi[uIndex])[0];
                uchCRCLo = auchCRCLo[uIndex];
            }

            bool res = (uchCRCHi == data[len]) && (uchCRCLo == data[len + 1]);

            return res;
        }

        public static void calcAndAppendCRC(ref byte[] PDU) //将PDU计算校验 并在后面添加校检字节
        {
            UInt16 crc = crc16(ref PDU);
            byte[] t = new byte[PDU.Length + 2];
            PDU.CopyTo(t, 0);
            t[PDU.Length] = BitConverter.GetBytes(crc)[1];
            t[PDU.Length + 1] = BitConverter.GetBytes(crc)[0];
            PDU = t;
        }

        public static void generateWriteRegsPDU(out byte[] outPDU, int slaveID, UInt16 addr, ref UInt16[] regs)
        {
            int len = 7 + 2 * regs.Length;
            outPDU = new byte[len];
            outPDU[0] = BitConverter.GetBytes(slaveID)[0];
            outPDU[1] = 0x10;
            outPDU[2] = BitConverter.GetBytes(addr)[1];
            outPDU[3] = BitConverter.GetBytes(addr)[0];
            outPDU[4] = BitConverter.GetBytes(regs.Length)[1];
            outPDU[5] = BitConverter.GetBytes(regs.Length)[0];
            outPDU[6] = BitConverter.GetBytes(regs.Length * 2)[0];
            // 7 byte header

            //append regs byte
            int i = 7;
            foreach (UInt16 r in regs)
            {
                outPDU[i++] = BitConverter.GetBytes(r)[1];
                outPDU[i++] = BitConverter.GetBytes(r)[0];
            }
        }
        public static void generateReadRegsPDU(out byte[] outPDU, int slaveID, UInt16 addr, UInt16 regCount)
        {
            int len = 6;
            outPDU = new byte[len];
            outPDU[0] = BitConverter.GetBytes(slaveID)[0];
            outPDU[1] = 0x03;
            outPDU[2] = BitConverter.GetBytes(addr)[1];
            outPDU[3] = BitConverter.GetBytes(addr)[0];
            outPDU[4] = BitConverter.GetBytes(regCount)[1];
            outPDU[5] = BitConverter.GetBytes(regCount)[0];
            // 6 byte header

            // crc
            calcAndAppendCRC(ref outPDU);
        }

        public static bool decodeReadRegsResponse(ref byte[] sendPDU, ref byte[] receivePDU, out UInt16[] regs)
        {
            bool isVaild = true;
            regs = null;

            //check header
            if (receivePDU.Length < 7 || sendPDU.Length < 8)
            {
                return false;
            }
            for (int i = 0; i < 2; ++i)
            {
                isVaild = isVaild && (sendPDU[i] == receivePDU[i]);
                if (!isVaild) return false;
            }

            //check crc
            if (!isPDU_CRC_OK(ref receivePDU)) return false;

            int cnt = receivePDU[2];
            int index = 3;
            cnt /= 2;
            if (cnt < 1) return false;
            UInt16[] res = new UInt16[cnt];
            byte[] t = new byte[2];
            for (int i = 0; i < cnt; ++i)
            {
                t[0] = receivePDU[index + 1];
                t[1] = receivePDU[index];
                res[i] = BitConverter.ToUInt16(t, 0);
                index += 2;
            }

            regs = res;

            return isVaild;
        }

        public static void generateWriteCoilsPDU(out byte[] outPDU, int slaveID, UInt16 addr, ref byte[] coils)
        {
            int senddatalen = coils.Length / 8;
            if ((coils.Length % 8) != 0) senddatalen++;
            int len = 7 + senddatalen;
            outPDU = new byte[len];
            outPDU[0] = BitConverter.GetBytes(slaveID)[0];
            outPDU[1] = 0x0f;
            outPDU[2] = BitConverter.GetBytes(addr)[1];
            outPDU[3] = BitConverter.GetBytes(addr)[0];
            outPDU[4] = BitConverter.GetBytes(coils.Length)[1];
            outPDU[5] = BitConverter.GetBytes(coils.Length)[0];
            outPDU[6] = BitConverter.GetBytes(senddatalen)[0];
            // 7 byte header

            //append coils byte
            int index = 7;
            for (int j = 0; j < outPDU[6]; ++j)
            {
                int b = 0;
                for (int i = 0; i < 8; ++i)
                {
                    if ((j * 8 + i) >= coils.Length) break;
                    if (coils[j * 8 + i] != 0)
                    {
                        b |= (0x01 << (i % 8));
                    }
                }
                outPDU[index] = BitConverter.GetBytes(b)[0];
                index++;
            }

            // crc
            calcAndAppendCRC(ref outPDU);
        }


    }//class ModbusHelp


}
